#!/usr/bin/env python

"""
Sensor Platform Simulation
==========================
This example looks at how platforms and sensors can be used within the Stone Soup simulation
capability.
"""

# %%
# Building a Simulated Sensor Platform
# ------------------------------------
# The focus of this example is to show how to set up and configure simulations. As such, the
# application of a tracker will not be covered in detail. For more information about trackers
# and how to configure them, review of the tutorials and demonstrations is recommended.
#
# This example makes use of Stone Soup :class:`~.FixedPlatform` and :class:`~.Sensor` objects.
#
# In order to configure platforms, sensors, and the simulation, we will need to import some specific
# Stone Soup objects. As these have been introduced in previous tutorials, they are imported
# upfront. New functionality within this example will be imported at the relevant point
# to draw attention to the new features.

# Some general imports and set up
from datetime import datetime
from datetime import timedelta

import numpy as np

# Stone Soup imports:
from stonesoup.types.state import State, GaussianState
from stonesoup.types.array import StateVector, CovarianceMatrix
from stonesoup.models.transition.linear import (
    CombinedLinearGaussianTransitionModel, ConstantVelocity)
from stonesoup.models.measurement.nonlinear import CartesianToElevationBearingRange
from stonesoup.updater.kalman import UnscentedKalmanUpdater
from stonesoup.predictor.kalman import UnscentedKalmanPredictor
from stonesoup.deleter.time import UpdateTimeStepsDeleter
from stonesoup.tracker.simple import MultiTargetTracker
from matplotlib import pyplot as plt

# Define the simulation start time
start_time = datetime.now().replace(microsecond=0)

# %%
# Create a Platform
# -----------------
# The first element we need to create is a platform. For this first example, we will build a static
# (or *fixed*) platform which is located at the origin. We are going to work in a 6-dimensional
# state space, and our platform will have the following :math:`\mathbf{x}`.
#
# .. math::
#           \mathbf{x} = \begin{bmatrix}
#                          x\\ \dot{x}\\ y\\ \dot{y}\\ z\\ \dot{z} \end{bmatrix}
#                      = \begin{bmatrix}
#                          0\\ 0\\ 0\\ 0\\ 0\\ 0 \end{bmatrix}
#
# Because the platform is static, we only need to define :math:`(x, y, z)`. Any internal
# interactions within the platform which requires knowledge of platform velocity
# :math:`(\dot{x}, \dot{y}, \dot{z})` will be returned :math:`(0, 0, 0)`.

# Firstly, import the fixed platform
from stonesoup.platform.base import FixedPlatform

# Define the initial platform position - in this case the origin
platform_state_vector = StateVector([[0], [0], [0]])
position_mapping = (0, 1, 2)

# Create the initial state (position, time)
platform_state = State(platform_state_vector, start_time)

# create our fixed platform
platform = FixedPlatform(states=platform_state,
                         position_mapping=position_mapping)

# %%
# We have now created a platform within Stone Soup and located it at the origin of our state space.
# As previously stated, the platform will have a velocity :math:`(\dot{x}, \dot{y}, \dot{z})` of
# :math:`(0, 0, 0)` which we can check:
platform.velocity

# %%
# We can also query the platform orientation:
platform.orientation

# %%
# Create a Sensor
# ---------------
# Now that we have a platform, the next step is to create a sensor which can be added to it.
# A radar will be created which is capable of measuring the range, bearing, and elevation of the
# target relative to the sensor.
#
# The :class:`~.RadarRangeBearingElevation` class provides a sensor wrapper around the
# :class:`~.CartesianToElevationBearingRange` measurement model. The measurement model provides a
# time-invariant measurement model where measurements are assumed to be received in the form of
# elevation (:math:`\theta`), bearing (:math:`\phi`), and range (:math:`r`) with Gaussian noise in
# each dimension.
#
# The model is described by the following equation:
#
# .. math::
#           \mathbf{z}_k = h(\mathbf{x}_k, \dot{\mathbf{x}}_k)
#
# where :math:`\mathbf{z}_k` is a measurement vector of the form:
#
# .. math::
#           \mathbf{z}_k = \begin{bmatrix} \theta \\ \phi \\ r \end{bmatrix}
#
# and :math:`h` is a non-linear model function of the form:
#
# .. math::
#           h(\mathbf{x}_k,\dot{\mathbf{x}}_k) = \begin{bmatrix}
#                     \arcsin{(\mathcal{z}/\sqrt{\mathcal{x}^2 + \mathcal{y}^2 +\mathcal{z}^2})} \\
#                     \arctan{(\mathcal{y},\mathcal{x})} \\
#                     \sqrt{\mathcal{x}^2 + \mathcal{y}^2 + \mathcal{z}^2}
#                     \end{bmatrix} + \dot{\mathbf{x}}_k
#
# and :math:`\mathbf{z}_k` is Gaussian distributed with covariance :math:`R`:
#
# .. math::
#           \mathbf{z}_k \sim \mathcal{N}(0,R)
#
# .. math::
#           R = \begin{bmatrix}
#             \sigma_{\theta}^2 & 0 & 0 \\
#             0 & \sigma_{\phi}^2 & 0 \\
#             0 & 0 & \sigma_{r}^2
#             \end{bmatrix}
#
# We now create our radar:

# Import a radar sensor model
from stonesoup.sensor.radar.radar import RadarElevationBearingRange

# First we need to configure a radar

# Generate a radar sensor with a suitable measurement accuracy
noise_covar = CovarianceMatrix(np.array(np.diag([np.deg2rad(3)**2,
                                                 np.deg2rad(0.15)**2,
                                                 25**2])))

# this radar measures range with an accuracy of +/- 25m, elevation accuracy +/- 3
# degrees, and bearing accuracy of +/- 0.15 degrees

# The radar needs to be informed of where x, y, and z are in the target state space
radar_mapping = (0, 2, 4)

# Instantiate the radar
radar = RadarElevationBearingRange(ndim_state=6,
                                   position_mapping=radar_mapping,
                                   noise_covar=noise_covar)
# %%
# Attach the sensor to the platform
# ---------------------------------
# Now that we have created our radar sensor, we need to mount the sensor onto our platform.
#
# Sensors can be mounted with two additional parameters: mounting offset and rotation offset.
#
# The mounting offset:
#
#  * defines how the sensors position is offset from the platform
#  * defaults to a position offset of zero.
#
# The rotation offset:
#
#  * defines the sensor's orientation relative to that of the platform
#  * defaults to an orientation offset of zero.
#
# The default assumption is that the sensor is located at the centre point of the platform and
# orientated to align with the platform body. Here, we are happy to use the default assumptions
# and therefore the sensor can be added.
platform.add_sensor(radar)

# %%
# As before, we can query the platform to demonstrate that it has a sensor mounted:
platform.sensors

# %%
# You will notice that ``platform.sensors`` returns a list which contains our single sensor.
# This hints at the multi-sensor platform functionality which is shown in a subsequent example.
#
# We can also check that the default mounting offsets have been applied:
radar.mounting_offset

# %%
# and that the rotation offsets have been applied:
radar.rotation_offset

# %%
# Building a simulation
# ---------------------
# Now that we have created a sensor platform, we need to build a simulation which generates targets
# for the sensor to detect and track. We are going to use a
# :class:`~.MultiTargetGroundTruthSimulator`. This simulator enables multiple ground truth targets
# to be created based on a number of user-defined parameters.
#
# In this example, targets are initiated with values based upon a mean state and a covariance,
# using a Gaussian assumption. This is done by creating a :class:`~.GaussianState` object which
# describes the distribution which we want our targets to be drawn from. Targets will be generated
# using the following parameters:
#
#  * :math:`x` is Gaussian-distributed around the platform location with variance of
#    :math:`\mathrm{2}km`.
#  * :math:`y` is Gaussian-distributed around the platform location with variance of
#    :math:`\mathrm{2}km`.
#  * :math:`z` is Gaussian-distributed around an altitude of :math:`\mathrm{9}km` with variance
#    of :math:`\mathrm{0.1}km`.
#  * :math:`\dot{x}` is Gaussian-distributed around :math:`\mathrm{100}ms^{-1}` with variance of
#    :math:`\mathrm{50}ms^{-1}`.
#  * :math:`\dot{y}` is Gaussian-distributed around :math:`\mathrm{100}ms^{-1}` with variance
#    of :math:`\mathrm{50}ms^{-1}`.
#  * :math:`\dot{z}` is Gaussian-distributed around :math:`\mathrm{0}ms^{-1}` with variance of
#    :math:`\mathrm{1}ms^{-1}`.
#
# We will also configure our simulator to randomly create and delete targets based on a specified
# birth rate and death rate. We set the birth rate to be 0.10 - on any given time step there is
# a 10% chance of a new target being initiated. The death rate has been set to 0.01 - on any given
# time step, there is a 1% chance that a target will be removed from the simulation.
#
# The above setup will provide a case which loosely approximates an air surveillance radar at an
# airport.
from stonesoup.simulator.simple import MultiTargetGroundTruthSimulator

# Set a constant velocity transition model for the targets
transition_model = CombinedLinearGaussianTransitionModel(
    [ConstantVelocity(0.5), ConstantVelocity(0.5), ConstantVelocity(0.1)])

# Define the Gaussian State from which new targets are sampled on initialisation
initial_target_state = GaussianState(StateVector([[0], [0], [0], [0], [9000], [0]]),
                                     CovarianceMatrix(np.diag([2000, 50, 2000, 50, 100, 1])))

groundtruth_sim = MultiTargetGroundTruthSimulator(
    transition_model=transition_model,  # target transition model
    initial_state=initial_target_state,  # add our initial state for targets
    timestep=timedelta(seconds=1),  # time between measurements
    number_steps=120,  # 2 minute
    birth_rate=0.10,  # 10% chance of a new target being born
    death_probability=0.01  # 1% chance of a target being killed
)

# %%
# With our ground truth simulation now set up, we need to add our platform into the simulation
# capability. This is done using the :class:`~.PlatformDetectionSimulator` class. This simulator
# allows a *list* of platforms to be added into the simulation. When the simulation is processed,
# the platforms are able to make detections of both the ground truth targets and other platforms.
#
# In this case we have a single platform. Therefore, the radar sensor will only be able to make
# measurements of the ground truth objects generated by the simulator.

# Import the PlatformDetectionSimulator
from stonesoup.simulator.platform import PlatformDetectionSimulator

sim = PlatformDetectionSimulator(groundtruth=groundtruth_sim, platforms=[platform])

# %%
# Creating the Tracker Components
# -------------------------------
# As stated above, the aim of this example is to show how the :class:`~.Platform`,
# :class:`~.Sensor`, and :class:`~.Simulator` classes work within Stone Soup. We will therefore
# quickly build an Unscented Kalman Filter which initiates measurements using a simple heuristic
# initiation and deletes any track where no detection is associated for 2 consecutive time steps.
# There are a number of tutorials for how to build the tracking components provided in the
# :ref:`auto_tutorials/index:Tutorials`.

# Create an Unscented Kalman Predictor
predictor = UnscentedKalmanPredictor(transition_model)

# Create an Unscented Kalman Updater
# Note our sensor adds a measurement model to detections
updater = UnscentedKalmanUpdater(measurement_model=None)

# %%
# When we build our updater, we do not provide a measurement model. This is because we have defined
# a measurement model which is attached to our radar sensor. Each detection made by this sensor
# will have our radar measurement model associated with it. In Stone Soup, the
# :class:`~.Updater` checks the detections provided and will use any measurement model attached
# to them.


# %%
# Setup Initiator class for the Tracker
# -------------------------------------
# We will now build a simple heuristic initiator. This assumes that most of the deviation is caused
# by the bearing measurement error. It converts the bearing error into :math:`x, y` components
# using the target bearing. For z, we simply use :math:`r*\sigma_{\theta}^2` (this ignores
# any bearing or range related components). Velocity covariances are just based on expected velocity
# range of targets.
from stonesoup.types.state import GaussianState
from stonesoup.types.update import GaussianStateUpdate
from stonesoup.initiator.simple import SimpleMeasurementInitiator
from stonesoup.types.track import Track
from stonesoup.types.hypothesis import SingleHypothesis


class Initiator(SimpleMeasurementInitiator):
    def initiate(self, detections, timestamp, **kwargs):
        max_dev = 500.
        tracks = set()
        measurement_model = self.measurement_model
        for detection in detections:
            state_vector = measurement_model.inverse_function(
                            detection)
            model_covar = measurement_model.covar()

            el_az_range = np.sqrt(np.diag(model_covar))  # elev, az, range

            std_pos = detection.state_vector[2, 0]*el_az_range[1]
            stdx = np.abs(std_pos*np.sin(el_az_range[1]))
            stdy = np.abs(std_pos*np.cos(el_az_range[1]))
            stdz = np.abs(detection.state_vector[2, 0]*el_az_range[0])
            if stdx > max_dev:
                print('Warning - X Deviation exceeds limit!!')
            if stdy > max_dev:
                print('Warning - Y Deviation exceeds limit!!')
            if stdz > max_dev:
                print('Warning - Z Deviation exceeds limit!!')
            c0 = np.diag(np.array([stdx, 50.0, stdy, 50.0, stdz, 10.0])**2)

            tracks.add(Track([GaussianStateUpdate(
                state_vector,
                c0,
                SingleHypothesis(None, detection),
                timestamp=detection.timestamp)
            ]))
        return tracks


meas_model = CartesianToElevationBearingRange(
            ndim_state=6,
            mapping=np.array([0, 2, 4]),
            noise_covar=noise_covar)

prior_state = GaussianState(
        np.array([[0], [0], [0], [0], [0], [0]]),
        np.diag([1000, 50, 1000, 50, 1000, 10.0])**2)

initiator = Initiator(prior_state, measurement_model=meas_model)

# %%
# Now that we have set up out tracking scenario, we can wrap our simulation environment within a
# :class:`~.MultiTargetTracker`. This combines our tracker configurations with the simulation we
# previously created into a single iterable object.

from stonesoup.hypothesiser.distance import DistanceHypothesiser
from stonesoup.measures import Mahalanobis
hypothesiser = DistanceHypothesiser(predictor, updater, measure=Mahalanobis(), missed_distance=3)

from stonesoup.dataassociator.neighbour import NearestNeighbour
data_associator = NearestNeighbour(hypothesiser)

deleter = UpdateTimeStepsDeleter(time_steps_since_update=2)

# Create a Kalman multi-target tracker
kalman_tracker = MultiTargetTracker(
    initiator=initiator,
    deleter=deleter,
    detector=sim,
    data_associator=data_associator,
    updater=updater
)

# %%
# The final step is to iterate our tracker over the simulation:
kalman_tracks = {}  # Store for plotting later
groundtruth_paths = {}  # Store for plotting later
detections = []  # Store for plotting later

for time, ctracks in kalman_tracker:
    for track in ctracks:
        loc = (track.state_vector[0], track.state_vector[2])
        if track not in kalman_tracks:
            kalman_tracks[track] = []
        kalman_tracks[track].append(loc)

    for truth in groundtruth_sim.current[1]:
        loc = (truth.state_vector[0], truth.state_vector[2])
        if truth not in groundtruth_paths:
            groundtruth_paths[truth] = []
        groundtruth_paths[truth].append(loc)

    for detection in sim.detections:
        detect_state = detection.measurement_model.inverse_function(detection)
        loc = (detect_state[0], detect_state[2])
        detections.append(loc)

# %%
# Plotting the outputs
# --------------------
# First we will plot the ground truth paths (red) which have been generated in the simulation step:
fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(1, 1, 1)
ax.set_xlabel("$East$")
ax.set_ylabel("$North$")
ax.set_ylim(-10000, 10000)
ax.set_xlim(-10000, 10000)

for key in groundtruth_paths:
    X = [coord[0] for coord in groundtruth_paths[key]]
    Y = [coord[1] for coord in groundtruth_paths[key]]
    ax.plot(X, Y, color='r')  # Plot true locations in red

# plot platform location
ax.scatter(0, 0, color='y')

# %%
# If we overlay the detections (black) onto the ground truth paths (red), we can see how the
# sensor performs, generating detections based upon the :class:`~.MeasurementModel` we provided it
# with. The platform location is shown in yellow:
fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(1, 1, 1)
ax.set_xlabel("$East$")
ax.set_ylabel("$North$")
ax.set_ylim(-10000, 10000)
ax.set_xlim(-10000, 10000)

for key in groundtruth_paths:
    X = [coord[0] for coord in groundtruth_paths[key]]
    Y = [coord[1] for coord in groundtruth_paths[key]]
    ax.plot(X, Y, color='r')  # plot true locations in red

X = [coord[0] for coord in detections]
Y = [coord[1] for coord in detections]
ax.scatter(X, Y, color='k')  # plot detections in black

# plot platform location
ax.scatter(0, 0, color='y')

# %%
# Now, we overlay the ground truth locations (red), detections (black) and tracks (blue). This
# shows all the stages of the tracker simulation we have built in a single figure.
# The platform location is shown in yellow:
fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(1, 1, 1)
ax.set_xlabel("$East$")
ax.set_ylabel("$North$")
ax.set_ylim(-10000, 10000)
ax.set_xlim(-10000, 10000)
for key in groundtruth_paths:
    X = [coord[0] for coord in groundtruth_paths[key]]
    Y = [coord[1] for coord in groundtruth_paths[key]]
    ax.plot(X, Y, color='r')  # Plot true locations in red

for key in kalman_tracks:
    X = [coord[0] for coord in kalman_tracks[key]]
    Y = [coord[1] for coord in kalman_tracks[key]]
    ax.plot(X, Y, color='b')  # Plot track estimates in blue

X = [coord[0] for coord in detections]
Y = [coord[1] for coord in detections]
ax.scatter(X, Y, color='k')  # Plot detections in black

# plot platform location
ax.scatter(0, 0, color='y')

# %%
# Finally, we can plot the estimated tracks (blue) alongside the ground truth paths (red).
# Because we used a noisy sensor, this view makes it easier to quickly see the tracker performance.
# The platform location is shown in yellow.
fig = plt.figure(figsize=(10, 6))
ax = fig.add_subplot(1, 1, 1)
ax.set_xlabel("$East$")
ax.set_ylabel("$North$")
ax.set_ylim(-10000, 10000)
ax.set_xlim(-10000, 10000)
for key in groundtruth_paths:
    X = [coord[0] for coord in groundtruth_paths[key]]
    Y = [coord[1] for coord in groundtruth_paths[key]]
    ax.plot(X, Y, color='r')  # Plot true locations in red

for key in kalman_tracks:
    X = [coord[0] for coord in kalman_tracks[key]]
    Y = [coord[1] for coord in kalman_tracks[key]]
    ax.plot(X, Y, color='b')  # Plot track estimates in blue

# plot platform location
ax.scatter(0, 0, color='y')

# sphinx_gallery_thumbnail_number = 3

# %%
# To familiarise yourself with sensors, it is recommended that you investigate changing the
# parameters within the sensor Measurement Model to see the impact on detections and ultimately
# tracker performance. We used a *hard* association logic coupled with a relatively noisy sensor.
# A suggested further exercise is to modify this example to use a *soft* association step such
# as Probabilistic Data Association (:class:`~.PDA`) or Joint Probabilistic Data Association
# (:class:`~.JPDA`).

# %%
# Key points
# ----------
# 1. Sensor platforms, which combine :class:`~.Sensor` and :class:`~.Platform` classes can be
# created in Stone Soup and used as part of a tracking simulation.
# 2. When using a :class:`~.Sensor` to generate detections, there is no need to provide an
# :class:`~.Updater` with a :class:`~.MeasurementModel` as each detection is attributed with the
# relevant sensors measurement model.
# 3. Sensors will generate detections of all platforms within the simulation, not just ground
# truth objects.
